﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using MSRoboticsFramework.ActuatorLayer.CompoundActuators;
using MSRoboticsFramework.ActuatorLayer.Actuators.Motors;

namespace WheeledTank.ActuatorLayer.Subsystems
{
    class TurretSubsystem : ITankTurret
    {
        private IGenericMotor HorizontalMotor;
        private IGenericMotor VerticalMotor;
        private IGenericMotor CannonMotor;

        private const float MAX_VERTICAL_ANGLE = Math.PI * 3 / 8;

        public TurretSubsystem(IGenericMotor HorizontalMotor, IGenericMotor VerticalMotor, IGenericMotor CannonMotor)
        {
            this.HorizontalMotor = HorizontalMotor;
            this.VerticalMotor = VerticalMotor;
            this.CannonMotor = CannonMotor;
        }

        #region ITankTurret Members

        public float HorizontalAngle
        {
            get
            {
                ///we can turn from -PI to PI, so translate that into a value from -1 to 1...
                return HorizontalMotor.Angle / Math.PI;
            }
            set
            {
                ///we can turn from -PI to PI, so translate value into an angle in radians...
                HorizontalMotor.Angle = value * Math.PI;
            }
        }

        public float HorizontalTurningSpeed
        {
            get
            {
                return HorizontalMotor.Speed;
            }
            set
            {
                HorizontalMotor.Speed = value;
            }
        }

        public void TurnLeft()
        {
            HorizontalMotor.Backward();
        }

        public void TurnRight()
        {
            HorizontalMotor.Forward();
        }

        public void StopHorizontalTurningAndBrake()
        {
            HorizontalMotor.Brake();
        }

        public float VerticalAngle
        {
            get
            {
                return VerticalMotor.Angle / MAX_VERTICAL_ANGLE;
            }
            set
            {
                ///translate value into a limiting value if needed...
                if(value < 0) value = 0;
                if(value > 1) value = 1;

                VerticalMotor.Angle = value * MAX_VERTICAL_ANGLE;
            }
        }

        public float VerticalTurningSpeed
        {
            get
            {
                return VerticalMotor.Speed;
            }
            set
            {
                VerticalMotor.Speed = value;
            }
        }

        public void TurnUp()
        {
            VerticalMotor.Forward();
        }

        public void TurnDown()
        {
            VerticalMotor.Backward();
        }

        public void StopVerticalTurningAndBrake()
        {
            VerticalMotor.Brake();
        }

        public void StopAndBrake()
        {
            VerticalMotor.Brake();
            HorizontalMotor.Brake();
        }

        public void FireAndReload()
        {
            throw new NotImplementedException();
        }

        #endregion
    }
}
